# pragma once

# include "cpp_tools_Headers.h"

namespace cpp_tools
{
    const double pi = M_PI; // 圆周率

    template <typename T>
    int sign(T value)
    {
        if (value == 0)
        {
            return 0;
        }
        else if (value > 0)
        {
            return 1;
        }
        else
        {
            return -1;
        }
    }

    /// @brief 向0值截断
    /// @param val 输入值
    /// @param val_cutoff 截断值
    /// @return 0,如果输入值的绝对值小于截断值，否则输入值的绝对值减少对应的截断值
    double cutoff(double val, const double val_cutoff = 0.01)
    {
        if (std::abs(val) <= val_cutoff)
        {
            val = 0;
        }
        else
        {
            val = val - val/std::abs(val)*val_cutoff;
        }

        return val;
    }

    /// @brief 限制上下界
    /// @param val 输入值
    /// @param lbv 下界
    /// @param ubv 上界
    /// @return 
    double saturation(double val, double lbv, double ubv)
    {
        return std::min(std::max(val,lbv),ubv);
    }

    // 实现与MATLAB的rem函数相同的功能
    double rem(double a, double b) {
        if (b == 0) {
            // 如果除数为0，返回NaN（与MATLAB的行为一致）
            return std::numeric_limits<double>::quiet_NaN();
        }
        return a - std::trunc(a / b) * b;
    };

    /// @brief 将角度规约到正负pi之间
    /// @param angle 角度，单位rad
    /// @return 
    double mod2pi(double angle)
    {
        angle = rem(angle,2*pi);
        if (angle < -pi)
        {
            angle = angle + 2*pi;
        }
        else if (angle > pi)
        {
            angle = angle - 2*pi;
        }
        
        return angle;
    };

    /// @brief 将xy坐标映射到给定道路上的曲率坐标中，适用于单次定位，如果给定的road不可用，则返回{nan,nan}
    /// @param road
    /// @param x 
    /// @param y 
    /// @return {s,e}
    template <typename T>
    std::pair<double,double> xy2sce_local(const T & road, double x, double y)
    {
        static_assert(std::is_member_object_pointer<decltype(&T::N)>::value, "Type must have 'N' member");
        static_assert(std::is_member_object_pointer<decltype(&T::x)>::value, "Type must have 'x' member");
        static_assert(std::is_member_object_pointer<decltype(&T::y)>::value, "Type must have 'y' member");
        static_assert(std::is_member_object_pointer<decltype(&T::S)>::value, "Type must have 'S' member");

        double s;
        double e;

        int N = road.N;
        if (N <= 0)
        {
            return {0./0.,0./0.};
        }

        Eigen::VectorXd dis = sqrt((road.x.array() - x).pow(2) + (road.y.array() - y).pow(2));

        Eigen::Index pointer;
        dis.minCoeff(&pointer);

        int search_flag;
        if (pointer == 0)
        {
            search_flag = 1;
        }
        else if (pointer == N - 1)
        {
            search_flag = -1;
        }
        else if (dis(pointer + 1) <= dis(pointer - 1))
        {
            search_flag = 1;
        }
        else
        {
            search_flag = -1;
        }

        if (search_flag == 1)
        {
            auto pointer_a = pointer;
            auto pointer_b = pointer + 1;

            double theta = std::atan2(y - road.y(pointer_a), x - road.x(pointer_a))
                - std::atan2(road.y(pointer_b) - road.y(pointer_a), road.x(pointer_b) - road.x(pointer_a));

            theta = mod2pi(theta);

            s = road.S(pointer_a) + dis(pointer_a)*std::cos(theta);
            e = dis(pointer_a)*std::sin(theta);
        }
        else
        {
            auto pointer_a = pointer - 1;
            auto pointer_b = pointer;

            auto theta = std::atan2(road.y(pointer_a) - road.y(pointer_b), road.x(pointer_a) - road.x(pointer_b))
                - std::atan2(y - road.y(pointer_b), x - road.x(pointer_b));

            theta = mod2pi(theta);

            s = road.S(pointer_b) - dis(pointer_b)*std::cos(theta);
            e = dis(pointer_b)*std::sin(theta);
        }
        
        return {s,e};
    };
} // namespace cpp_tools
